#[derive(Debug)]
#[cfg_attr(feature = "defmt", derive(defmt::Format))]
pub enum Dir {
    CCW,
    CW,
    Brake,
}

pub trait MotorDrv {
    type Error;

    const TIMEOUT_ERR: Self::Error;
    const ALM_ERR: Self::Error;

    fn is_ccw_origin() -> bool;
    fn is_cw_origin() -> bool;
    fn delay(tick: u32);

    const CHECK_TICK: u32 = 20; // 循环检测间隔
    const ALM_DELAY_TICK: u32 = 500; // 延迟xx之后 在开始检测堵转
    const TIMEOUT_DURATION: u32 = 10 * 1000; // 超时时间
    const ALM_VALUE: u16 = 500; // 堵转最大值
    const ALM_COUNT: u32 = 5; // 连续检测堵转 次数

    fn analog_value() -> u16;
    fn set_dir(dir: Dir);
}

pub struct Motor<T: MotorDrv> {
    _drv: T,
}

impl<T: MotorDrv> Motor<T> {

    pub fn run_to_cw_origin() -> Result<(), T::Error> {
        if T::is_cw_origin() {
            return Ok(());
        }
        let mut alm_count = 0;
        let mut time_count = 0;
        let mut ret = Ok(());
        T::set_dir(Dir::CW);

        loop {
            if T::is_cw_origin() {
                break;
            }

            if time_count >= T::ALM_DELAY_TICK {
                if T::analog_value() >= T::ALM_VALUE {
                    alm_count += 1;
                    if alm_count >= T::ALM_COUNT {
                        ret = Err(T::ALM_ERR);
                        break;
                    }
                } else {
                    alm_count = 0;
                }
            }

            T::delay(T::CHECK_TICK);
            time_count += T::CHECK_TICK;
            if time_count >= T::TIMEOUT_DURATION {
                ret = Err(T::TIMEOUT_ERR);
                break;
            }
        }
        T::set_dir(Dir::Brake);
        ret
    }

    pub fn run_to_ccw_origin() -> Result<(), T::Error> {
        if T::is_ccw_origin() {
            return Ok(());
        }
        let mut alm_count = 0;
        let mut time_count = 0;
        let mut ret = Ok(());
        T::set_dir(Dir::CCW);

        loop {
            if T::is_ccw_origin() {
                break;
            }

            if time_count >= T::ALM_DELAY_TICK {
                if T::analog_value() >= T::ALM_VALUE {
                    alm_count += 1;
                    if alm_count >= T::ALM_COUNT {
                        ret = Err(T::ALM_ERR);
                        break;
                    }
                } else {
                    alm_count = 0;
                }
            }

            T::delay(T::CHECK_TICK);
            time_count += T::CHECK_TICK;
            if time_count >= T::TIMEOUT_DURATION {
                ret = Err(T::TIMEOUT_ERR);
                break;
            }
        }
        T::set_dir(Dir::Brake);
        ret
    }
}
